#include <stdio.h>
#include <common/Thread.h>
#include "eventmanager.h"
#include "route.h"
#include "parammanager.h"
#include "lockablestorage.h"
#include "properties.h"
#include "states/state_main.h"
#include "states/stateid.h"
#include "states/statesave.h"
#include "slowloop.h"

SlowLoop::SlowLoop(EventManager *evtmgr, Route *route, ParamManager *parammgr, LockableStorage *storage)
	: evtmgr_(evtmgr)
	, route_(route)
	, parammgr_(parammgr)
	, queue_(16, sizeof(int))
	, fsm_(new state::Main())
	, events_(16)
	, statesave_(storage)
{}

SlowLoop::~SlowLoop() {
	delete fsm_;
}

void SlowLoop::run() {
	Period period(200);	// thread period is 200 ms
	
	init();

	while (!isExitFlag()) {
		route_->update();
		checkEvents();
		processEvents();
		doStateSave();
		period.wait();
	}
}

void SlowLoop::init() {
	evtmgr_->attach(&queue_);

	bool restored = false;//restoreFSM();
	fsm_->enter(restored);
	restoreRoute();
}

void SlowLoop::doStateSave() {
	int index = route_->getActiveIndex();
	statesave_.setWptIndex(index);

	PropAccessor p;
	statesave_.setId(p().fcs.state);
}

bool SlowLoop::restoreFSM() {
	int id = statesave_.getId();
	if (id == stateid::MANUAL_RC) {
		PropAccessor p;
		p().fcs.manual_rc = true;
	}

	return (id >= 0) ? fsm_->setSubstateById(id) : false;
}

void SlowLoop::restoreRoute() {
	int index = statesave_.getWptIndex();
	if (index >= 0)
		route_->flyTo(index);
}

void SlowLoop::checkEvents() {
	events_.erase();
	fsm_->getActiveEvents(events_);
	evtmgr_->check(events_);
}

void SlowLoop::processEvents() {
	int evt;
	state::State *newState = NULL;
	while (queue_.get(&evt, 0))
		fsm_->doEvent(evt, newState);
}
